#! /usr/bin/env python

import rospy
import std_msgs
import geometry_msgs.msg
from geometry_msgs.msg import Pose
from geometry_msgs.msg import PoseStamped

import lcm
from mani_lcm_msgs import touch_point_msg

ROS_PUBLISH_TOPIC = "touch_point_cmd"

class LCMHandler():
    def __init__(self):

        self.ros_pub = rospy.Publisher(ROS_PUBLISH_TOPIC, PoseStamped, queue_size = 1)
        rospy.init_node('touch_point_ros_router')

        pass

    def touch_point_msg_handler(self, channel, data):
        print 'lcm incoming', channel
        msg = touch_point_msg.decode(data)
        print 'lcm_msg: \n', msg.p, msg.w
        ros_msg = PoseStamped()
        ros_msg.header = std_msgs.msg.Header()
        ros_msg.header.stamp = rospy.Time.now();

        ros_msg.pose = Pose()

        ros_msg.pose.position.x = msg.p[0]
        ros_msg.pose.position.y = msg.p[1]
        ros_msg.pose.position.z = msg.p[2]

        ros_msg.pose.orientation.x = msg.w[0]
        ros_msg.pose.orientation.y = msg.w[1]
        ros_msg.pose.orientation.z = msg.w[2]
        ros_msg.pose.orientation.w = msg.w[3]

        self.ros_pub.publish(ros_msg)
        pass

LCM_CHANNEL_NAME = "TOUCH_POINT"

if __name__ == "__main__":
    print 'Initializing ',  'touch_point_ros_router'
    # init rospy

    # init lcm connection
    lc = lcm.LCM()
    lcm_msg_handler = LCMHandler()
    subscription = lc.subscribe(LCM_CHANNEL_NAME, lcm_msg_handler.touch_point_msg_handler)

    print 'Initilized, running'

    try:
        while not rospy.is_shutdown():
            lc.handle()
            rospy.sleep(1)
    except KeyboardInterrupt:
        pass
    except rospy.ROSInterruptException:
        pass

    pass
